from mvccep_image_localization.msg import base_localization
import rospy  
import math

"""
获取小车的起始位置,任意车号均可,只需传入被控小车id(0,1,2,3,4)
"""
        
def startP_sub(robot_id):
    # 被其他python程序import时，注释下行 ：一个python文件只能有一条rospy.init_node()命令
    # rospy.init_node("position_sub_node", anonymous=False)
    msg = rospy.wait_for_message("base_localization", base_localization, timeout=None)
    robot_list = [[msg.robot_0.pos.x, msg.robot_0.pos.y, msg.robot_0.angle], 
                  [msg.robot_1.pos.x, msg.robot_1.pos.y, msg.robot_1.angle],
                  [msg.robot_2.pos.x, msg.robot_2.pos.y, msg.robot_2.angle],
                  [msg.robot_3.pos.x, msg.robot_3.pos.y, msg.robot_3.angle],
                  [msg.robot_4.pos.x, msg.robot_4.pos.y, msg.robot_4.angle]]
    # b = round(a, 2) 将a保留两位小数并赋值给b
    startP = [round(robot_list[robot_id][0], 2), round(robot_list[robot_id][1], 2), round(robot_list[robot_id][2], 2)]
    
    return startP
        
if __name__ == '__main__':
    startP = startP_sub()
    print("*** 车辆起始点: ", startP)
